extern "C"{
#include "rm_dynamics_arm.h"
}
#include <ros/ros.h>
#include <dual_arm_msgs/MoveJ.h>
#include <dual_arm_msgs/Plan_State.h>

 

int main(int argc, char **argv)
{

	// 初始化ROS节点
    ros::init(argc, argv, "inverse_kinematics_demo");
    // 创建节点句柄
    ros::NodeHandle nh;
    
    //初始化函数
    RM_Init();
	//机械臂安装角度，右臂为90，-90度；左臂为-90，-90度，输入参数需转换弧度
	setAngle(-1.57,-1.57); 
	
	KINEMATIC q_pose;
	
	
	// 空间规划指令Publisher
    ros::Publisher moveJ_left_pub = nh.advertise<dual_arm_msgs::MoveJ>("/l_arm/MoveJ_Cmd", 10);

    ros::Duration(2.0).sleep();


	float joint[7];
	joint[0] = 0;
	joint[1] = 0;
	joint[2] = 0;
	joint[3] = -60;
	joint[4] = 0;
	joint[5] = -20;
	joint[6] = 0;
	
	
	//in_pose为参考位姿,数值越精确，逆解值越接近目标位姿
	KINEMATIC in_pose;
	/*
	in_pose.pose.x = 0;
	in_pose.pose.y = 0.6265;
	in_pose.pose.z = -0.32367;
	in_pose.ort.rx = 3.14159;
	in_pose.ort.ry = -0.17453;
	in_pose.ort.rz = 1.57079;
	*/
	in_pose = Forward_Kinematics(joint);

	//q_out为逆解输出的角度值
	float q_out[6];
	uint8_t flag = 1;
	
    
    //调用机械臂运动学逆解api，通过目标位姿，获取目标角度。
    //joint为参考角度（该角度距离目标角度越相近越好），in_pose为目标位姿，q_out为逆解结果，flag为1代表in_pose为欧拉角形式的位姿
    inverse_kinematics(joint, &in_pose, q_out, flag);
    ROS_INFO("inverse kinematics joint is:[%f, %f, %f, %f, %f, %f, %f]",q_out[0], q_out[1], q_out[2], q_out[3], q_out[4], q_out[5], q_out[6]);
    
	ros::Duration(1.0).sleep();
	
	//定义一个MoveJ指令的目标位姿
    dual_arm_msgs::MoveJ moveJ_target;
    moveJ_target.joint[0] = q_out[0]/180*3.14;
    moveJ_target.joint[1] = q_out[1]/180*3.14;
    moveJ_target.joint[2] = q_out[2]/180*3.14;
    moveJ_target.joint[3] = q_out[3]/180*3.14;
    moveJ_target.joint[4] = q_out[4]/180*3.14;
    moveJ_target.joint[5] = q_out[5]/180*3.14;
    moveJ_target.joint[6] = q_out[6]/180*3.14;
    moveJ_target.speed = 0.3;


    //发布空间规划指令使机械臂运动到目标位姿
    moveJ_left_pub.publish(moveJ_target);
    
	ros::waitForShutdown();
    return 0;
}
